#include "mt6701ct_spi.h"
#include <math.h>  // 添加这一行
#include "spi.h"

int rotationCount = 0;  // 旋转过的圈数
int rotationCount_Last; // 上一次循环时转过的圈数
float angleLast;    // 保持一致

float totalAngle = 0.0f;  // 累计总角度（可正可负）
float lastAngle = 0.0f;   // 上一次的角度值

extern SPI_HandleTypeDef hspi1;
/******************************/
//函数名：MT6701_GetRawData
//参  数：uint16_t* rawData
//返回值：0代表，没有错误。1，2为error
//作  用：使用SPI获取mt6701的原始数据
//作  者：
//时  间：
/******************************/
int MT6701_GetRawData(uint16_t* rawData){
    uint16_t txData = 0xFFFF;
    uint16_t timeout = 200;

    while(HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY){
        if(timeout-- ==0){
            return 1;//超时直接返回，避免执行后面代码
        }
    }
    MT6701_CS_Enable();
    HAL_StatusTypeDef spiStatus = HAL_SPI_TransmitReceive(&hspi1,(uint8_t *)&txData,(uint8_t *)rawData,1, HAL_MAX_DELAY);
    if(spiStatus != HAL_OK){
        MT6701_CS_Disable();
        return 2;//在SPI传输错误的时候，避免执行后面代码
    }
    MT6701_CS_Disable();
    *rawData >>= 2;//右移动两位，取出高位置14位角度原始数据
    return 0;
}

/******************************/
//函数名：MT6701_GetRawAngle
//参  数：
//返回值：
//作  用：对RawData进行处理，得到无圈数累加的原始角度
//作  者：
//时  间：
/******************************/
float MT6701_GetRawAngle(void){
    uint16_t rawData;
    int a =5;
    a = MT6701_GetRawData(&rawData);
    return (float)rawData / 16384.0f * 360;
}

/**
* 统计旋转的圈数
*/
float MT6701_GetTotalRotations() {
    float currentAngle = MT6701_GetRawAngle();  // 0-360°
    float deltaAngle = currentAngle - lastAngle;

    // 处理角度跨越 0° 的情况
    if (deltaAngle > 180.0f) deltaAngle -= 360.0f;
    else if (deltaAngle < -180.0f) deltaAngle += 360.0f;

    totalAngle += deltaAngle;  // 累加角度变化
    lastAngle = currentAngle;  // 更新上次角度

    return totalAngle / 360.0f;  // 返回总圈数（如 1.2、3.7）
}

float last_angle = 0;
uint32_t last_time = 0;

float get_motor_speed_rpm()
{
    float current_angle = MT6701_GetRawAngle(); // 当前角度（单位：度，范围 0~360）
    uint32_t now = HAL_GetTick();               // 当前时间（单位：ms）

    float delta_angle = current_angle - last_angle;
    if (delta_angle < 0) delta_angle += 360.0f; // 处理回零重置

    uint32_t delta_time = now - last_time;      // 时间差（单位：ms）

    last_angle = current_angle;
    last_time = now;

    // deg/ms → RPM = (deg/ms) * (60秒/分钟) * (1圈/360度)
    float speed_rpm = (delta_angle / delta_time) * (1000.0f / 360.0f) * 60.0f;
    return speed_rpm;
}


float get_motor_speed_rps()
{
    float current_angle = MT6701_GetRawAngle(); // 当前角度（单位：度，范围 0~360）
    uint32_t now = HAL_GetTick();               // 当前时间（单位：ms）

    float delta_angle = current_angle - last_angle;
    if (delta_angle < 0) delta_angle += 360.0f; // 处理回零重置

    uint32_t delta_time = now - last_time;      // 时间差（单位：ms）

    last_angle = current_angle;
    last_time = now;

    // deg/ms → RPM = (deg/ms) * (60秒/分钟) * (1圈/360度)
    float speed_rpm = (delta_angle / delta_time) * (1000.0f / 360.0f);
    return speed_rpm;
}
